#include "Protocol_impl.hpp"
#include "time.h"
#include "stdlib.h"
#include "string.h"
#include "stdio.h"

#include "Configuration.hpp"
#include "BLE.hpp"
#include "indicator.hpp"
#include "updater.h"
#include "image_loader.h"
#include "conf.h"
#include "apm32a10x_fmc.h"
#include "MotorController.hpp"
#include "eeprom_device.hpp"
#include "fault_info.hpp"
#include "PiecewiseLinearInterpolation.hpp"
#include "MotorController.hpp"
extern FaultInfo *faultinfo;
extern unsigned int SystemTick;

extern  struct SystemConfiguration system_configuration;
extern  struct MotionCharacteristicData system_motion_characteristic_data;

extern  SystemConfiguration default_system_configuration;
extern  MotionCharacteristicData default_motion_characteristic_data;

extern BLE * ble;
extern LED * led;
extern Beeper * beeper;
extern MotorController *motor_controller;

bool inc_data_sync = false;
int authenticate_impl(char * password){
    uint8_t device_password[8]={0x01,0x02,0x03,0x04,0x05,0x06,0x07,0x08};
    for(int i=0; i<8; i++){
        if(device_password[i]!=password[i]){
            return 0;
        }
    }
    return 1;
}

uint16_t randKey_impl(){

    srand(SystemTick);
    uint16_t random_key = rand() &0xFFFF;
    return  random_key;
}

float floatdata = 0.0;
int sendResponse_impl(void *buffer,int size){
    uint8_t *buff = (uint8_t*)buffer;

    //printf("Response: %d\r\n",size);
    
    // for(int i=0; i < size; i++){
    //     printf("%x ",buff[i]);

    // }
    // printf("\r\nResponse finished\r\n");

    // if (size >= 4) {
    //     // 将最后四个字节赋值给 floatdata
    //     floatdata = *reinterpret_cast<float*>(buff + size - 4);
    // }
    // printf("read_position %f\r\n", floatdata);
    ble->write(buff,size);

   return 0;
}

unsigned char* byte_pointer;
int readData_impl(int index,void *buffer,int *size){
    switch(index){
        /*position */
        case motor_position:
            memcpy(buffer, &system_configuration.motor_position, sizeof(system_configuration.motor_position));
            *size = sizeof(system_configuration.motor_position);
            break;
        /*Minimum position*/
        case Minimum_position:
            //Configurator::load();
            data_load(Minimum_position);
            memcpy(buffer, &system_configuration.motor_position_minimum, sizeof(system_configuration.motor_position_minimum));
            *size = sizeof(system_configuration.motor_position_minimum);
            break;
        /*Maximum position*/
        case Maximum_position:
            //Configurator::load();
            data_load(Maximum_position);
            memcpy(buffer, &system_configuration.motor_position_maximum, sizeof(system_configuration.motor_position_maximum));
            *size = sizeof(system_configuration.motor_position_maximum);   
            break;
        /*Motion margin*/
        case Motion_margin:
            //Configurator::load();
            
            memcpy(buffer, &system_configuration.motor_motion_margin, sizeof(system_configuration.motor_motion_margin));
            *size = sizeof(system_configuration.motor_motion_margin);   
            break;
        /*Device serial number*/
        case Device_serial_number: 
            //Configurator::load();  
            data_load(Device_serial_number);     
            memcpy(buffer, system_configuration.device_serial_number, sizeof(system_configuration.device_serial_number));
            *size = sizeof(system_configuration.device_serial_number);
            break;
        /*Device password*/
        case Device_password:   
            //Configurator::load();
            data_load(Device_password);     
            memcpy(buffer, system_configuration.device_password, sizeof(system_configuration.device_password));
            *size = sizeof(system_configuration.device_password);
            break;
        /*Firmware version*/
        case Firmware_version:
            //Configurator::load();

            memcpy(buffer, system_configuration.firmware_version, sizeof(system_configuration.firmware_version));
            *size = sizeof(system_configuration.firmware_version);
            break;
        case inputduty_switch:
            //Configurator::load();
            data_load(inputduty_switch);
            memcpy(buffer, &system_configuration.inputduty_switch,sizeof(system_configuration.inputduty_switch));
            *size = sizeof(system_configuration.inputduty_switch);
            break;     
        case position_switch:
            //Configurator::load();
            data_load(position_switch);
            memcpy(buffer, &system_configuration.position_switch,sizeof(system_configuration.position_switch));
            *size = sizeof(system_configuration.position_switch);
            break;  
        case calibration_switch:
            //Configurator::load();
            data_load(calibration_switch);
            memcpy(buffer, &system_configuration.calibration_switch,sizeof(system_configuration.calibration_switch));
            *size = sizeof(system_configuration.calibration_switch);
            break;    
        case chip_id:
            //Configurator::load();
            data_load(chip_id);
            memcpy(buffer, system_configuration.chip_id,sizeof(system_configuration.chip_id));
            *size = sizeof(system_configuration.chip_id);
            break;               
        case bluetooth_switch:
            data_load(bluetooth_switch);
            memcpy(buffer, &system_configuration.bluetooth_switch,sizeof(system_configuration.bluetooth_switch));
            *size = sizeof(system_configuration.bluetooth_switch);
            break;     

        /*n_data_points*/
        case n_data_points:
            //Configurator::load();
            data_load(n_data_points);
            *(int *)buffer= system_motion_characteristic_data.n_data_points;
            *size = sizeof(system_motion_characteristic_data.n_data_points);
            break;
        /*x_data*/
        case x_data:
            //Configurator::load();
            data_load(x_data);
            memcpy(buffer, system_motion_characteristic_data.x_data, system_motion_characteristic_data.n_data_points * sizeof(system_motion_characteristic_data.x_data[0]));
            *size = system_motion_characteristic_data.n_data_points * sizeof(system_motion_characteristic_data.x_data[0]);
            
            break;
        /*y_data*/
        case y_data:
            //Configurator::load();
            data_load(y_data);
            memcpy(buffer, system_motion_characteristic_data.y_data, system_motion_characteristic_data.n_data_points * sizeof(system_motion_characteristic_data.y_data[0]));
            *size = system_motion_characteristic_data.n_data_points * sizeof(system_motion_characteristic_data.y_data[0]);
            break;                        
        default:
            *(int*)buffer = 0xABCDABCD;
            *size = sizeof(int);
            break;
    }
    return 0;
}

extern bool motormin_init_finished;   //电机最短极限位置初始化完成标志
extern bool motormax_init_finished;   //电机最长极限位置初始化完成标志
extern bool init_finished;
int  writeData_impl(int index,void *buffer,int size){
    switch(index){
        case Minimum_position:
            if(size<=sizeof(system_configuration.motor_position_minimum)){
                memcpy(&system_configuration.motor_position_minimum,buffer,size);
            }
            data_sync(Minimum_position,&system_configuration.motor_position_minimum);
            break;
        case Maximum_position:
            if((unsigned int)size<=sizeof(system_configuration.motor_position_maximum)){
                memcpy(&system_configuration.motor_position_maximum,buffer,size);
            }
            data_sync(Maximum_position,&system_configuration.motor_position_maximum);
            break;
        case Motion_margin:
            if(size<=sizeof(system_configuration.motor_motion_margin)){
                memcpy(&system_configuration.motor_motion_margin,buffer,size);
            }
            data_sync(Motion_margin,&system_configuration.motor_motion_margin);
            break;
        case Device_serial_number:
            if(size<=sizeof(system_configuration.device_serial_number)){
                memcpy(system_configuration.device_serial_number,buffer,size);
            }
            data_sync(Device_serial_number,&system_configuration.device_serial_number);      
            break;
        case Device_password:
            if(size<=sizeof(system_configuration.device_password)){
                memcpy(system_configuration.device_password,buffer,size);
            }          
            data_sync(Device_password,&system_configuration.device_password);
            break;
        case Firmware_version:
            if(size<=sizeof(system_configuration.firmware_version)){
                memcpy(system_configuration.firmware_version,buffer,size);
            }
            data_sync(Firmware_version,&system_configuration.firmware_version);
            break;
         case inputduty_switch:
            if(size<=sizeof(system_configuration.inputduty_switch)){
                memcpy(&system_configuration.inputduty_switch,buffer,size);
            }
            data_sync(inputduty_switch,&system_configuration.inputduty_switch);
            break;
         case position_switch:
            if(size<=sizeof(system_configuration.position_switch)){
                memcpy(&system_configuration.position_switch,buffer,size);
            }
            data_sync(position_switch,&system_configuration.position_switch);
            break;
         case calibration_switch:
            if(size<=sizeof(system_configuration.calibration_switch)){
                memcpy(&system_configuration.calibration_switch,buffer,size);
            }
            data_sync(calibration_switch,&system_configuration.calibration_switch);
            break;
         case bluetooth_switch:
            if(size<=sizeof(system_configuration.bluetooth_switch)){
                memcpy(&system_configuration.bluetooth_switch,buffer,size);
            }
            data_sync(bluetooth_switch,&system_configuration.bluetooth_switch);
            /* 控制蓝牙开关 */
            if(system_configuration.bluetooth_switch){
                ble->open();
                led->set(true);
            }else{
                beeper->beeper_time(1);
                ble->close();
                led->set(false);
            }
            break;

        case n_data_points:
            system_motion_characteristic_data.n_data_points = *(int*)(buffer);
            data_sync(n_data_points,&system_motion_characteristic_data.n_data_points);
            data_load(n_data_points); // 同步数据到RAM
            break;
        case x_data:
            if(size<=sizeof(system_motion_characteristic_data.x_data)){
                memcpy(system_motion_characteristic_data.x_data,buffer,size);
            }
            /* 数据检查 */
            for(int i = 0; i < size; i++){
                if(system_motion_characteristic_data.x_data[i] < 0) system_motion_characteristic_data.x_data[i] = 0;
                if(system_motion_characteristic_data.x_data[i] > 1) system_motion_characteristic_data.x_data[i] = 1;
            }     
            data_sync(x_data,&system_motion_characteristic_data.x_data);    //  写入数据到EEPROOM
            data_load(x_data); // 同步数据到RAM
            break;
        case y_data:
            if(size<=sizeof(system_motion_characteristic_data.y_data)){
                memcpy(system_motion_characteristic_data.y_data,buffer,size);
            }
            /* 数据检查 */
            for(int i = 0; i < size; i++){
                if(system_motion_characteristic_data.y_data[i] < 0) system_motion_characteristic_data.y_data[i] = 0;
                if(system_motion_characteristic_data.y_data[i] > 1) system_motion_characteristic_data.y_data[i] = 1;
            }     
            data_sync(y_data,&system_motion_characteristic_data.y_data);    //  写入数据到EEPROOM
            data_load(y_data);  // 同步数据到RAM
            break;
        case position_correct:
            if(init_finished)init_finished = false;
            motormin_init_finished = false;
            motormax_init_finished = false;

            break;
        case set_default:
            init_sync();
            init_load();
            NVIC_SystemReset();
            break;
        case interpolation_data_sync:
            printf("updatePli\r\n");
            motor_controller->updatePli();
            break;
        default:
            *(int*)buffer = 0xABCDABCD;
            break;
    }
    return 0;
}

int startOTA_impl(uint32_t file_size){
    int ret = 0;
    uint32_t target_address = OTA_AREA_ADDRESS;
    FMC_Unlock();
    ret = Updater_eraseFlash((void*)target_address,file_size);
    if(ret==file_size)printf("erase success\r\n");
    else return 1;
    FMC_Lock();
    return 0;
}

int writeOTAData_impl(uint32_t address_offset,void *buffer,int size){
    int ret = ble_writeOTAData(address_offset,buffer,size);
    /* flash error */
    if( ret == 1){
        faultinfo->set_error(flash_error);
        Send_DeviceFault_Info(flash_error);
    }

    return 0;
}

int verifyOTA_impl(void){
    uint32_t OTA_ADDRESS = OTA_AREA_ADDRESS;
    ImageHeader_t * iamge = (ImageHeader_t *)OTA_ADDRESS;
    int ret=ImageLoader_checkImage(iamge);
    if(ret==IL_OK){
        printf("ImageLoader_checkImage OK\r\n");
        NVIC_SystemReset();
    }
    else{
        Send_DeviceFault_Info(verify_error);
        printf("ImageLoader_checkImage failed\r\n");
        ret=Updater_eraseFlash((ImageHeader_t *)OTA_ADDRESS,iamge->length);        
        return 1;
    }
    return 0;
}


bool data_sync(int index,void *data){
    uint32_t address = 0x00000000;
    uint32_t offset = 0x00000000;
    EEPROM *eeprom = getEEPROM();
    eeprom->open();
        
    if ( eeprom == nullptr ) return false;
    switch (index)
    {
    case motor_position:
        offset = (unsigned int)(&system_configuration.motor_position) - (unsigned int)(&system_configuration);
        eeprom->write(static_cast<uint8_t*>(data),address+offset,sizeof(system_configuration.motor_position));
        break;
    case Minimum_position:
        offset = (unsigned int)(&system_configuration.motor_position_minimum) - (unsigned int)(&system_configuration);
        eeprom->write(static_cast<uint8_t*>(data),address+offset,sizeof(system_configuration.motor_position_minimum));        
        break;
    case Maximum_position:
        offset = (unsigned int)(&system_configuration.motor_position_maximum) - (unsigned int)(&system_configuration);
        eeprom->write(static_cast<uint8_t*>(data),address+offset,sizeof(system_configuration.motor_position_maximum));        
        break;
    case Device_serial_number:
        offset = (unsigned int)(&system_configuration.device_serial_number) - (unsigned int)(&system_configuration);
        eeprom->write(static_cast<uint8_t*>(data),address+offset,sizeof(system_configuration.device_serial_number));        
        break;
    case Device_password:
        offset = (unsigned int)(&system_configuration.device_password) - (unsigned int)(&system_configuration);
        eeprom->write(static_cast<uint8_t*>(data),address+offset,sizeof(system_configuration.device_password));        
        break;
    case Firmware_version:
        offset = (unsigned int)(&system_configuration.firmware_version) - (unsigned int)(&system_configuration);
        eeprom->write(static_cast<uint8_t*>(data),address+offset,sizeof(system_configuration.firmware_version));        
        break;
    case inputduty_switch:
        offset = (unsigned int)(&system_configuration.inputduty_switch) - (unsigned int)(&system_configuration);
        eeprom->write(static_cast<uint8_t*>(data),address+offset,sizeof(system_configuration.inputduty_switch));    
        break;
    case position_switch:
        offset = (unsigned int)(&system_configuration.position_switch) - (unsigned int)(&system_configuration);
        eeprom->write(static_cast<uint8_t*>(data),address+offset,sizeof(system_configuration.position_switch));        
        break;
    case calibration_switch:
        offset = (unsigned int)(&system_configuration.calibration_switch) - (unsigned int)(&system_configuration);
        eeprom->write(static_cast<uint8_t*>(data),address+offset,sizeof(system_configuration.calibration_switch));        
        break;
    case bluetooth_switch:
        offset = (unsigned int)(&system_configuration.bluetooth_switch) - (unsigned int)(&system_configuration);
        eeprom->write(static_cast<uint8_t*>(data),address+offset,sizeof(system_configuration.bluetooth_switch));        
        break;


    case n_data_points:
        offset = (unsigned int)(&system_motion_characteristic_data.n_data_points) - (unsigned int)(&system_motion_characteristic_data);
        eeprom->write(static_cast<uint8_t*>(data),address + sizeof(system_configuration) +offset,sizeof(system_motion_characteristic_data.n_data_points));        
        break;
    case x_data:
        offset = (unsigned int)(&system_motion_characteristic_data.x_data) - (unsigned int)(&system_motion_characteristic_data);
        eeprom->write(static_cast<uint8_t*>(data),address + sizeof(system_configuration) +offset,sizeof(system_motion_characteristic_data.x_data));        
        break;
    case y_data:
        offset = (unsigned int)(&system_motion_characteristic_data.y_data) - (unsigned int)(&system_motion_characteristic_data);
        eeprom->write(static_cast<uint8_t*>(data),address + sizeof(system_configuration) + offset,sizeof(system_motion_characteristic_data.y_data));        
        break;
    default:
        break;
    }
    eeprom->close();
    return true;
}

bool data_load(int index){
    uint32_t address = 0x00000000;
    uint32_t offset = 0x00000000;
    EEPROM *eeprom = getEEPROM();
    eeprom->open();
    if ( eeprom == nullptr ) return false;
    switch (index)
    {
    case Minimum_position:
        offset = (unsigned int)(&system_configuration.motor_position_minimum) - (unsigned int)(&system_configuration);
        eeprom->read(&system_configuration.motor_position_minimum,address+offset,sizeof(system_configuration.motor_position_minimum));
        //printf("system_configuration.motor_position_minimum %f\r\n",system_configuration.motor_position_minimum);
        break;
    case Maximum_position:
        offset = (unsigned int)(&system_configuration.motor_position_maximum) - (unsigned int)(&system_configuration);
        eeprom->read(&system_configuration.motor_position_maximum,address+offset,sizeof(system_configuration.motor_position_maximum));
        //printf("system_configuration.motor_position_maximum %f\r\n",system_configuration.motor_position_maximum);
        break;
    case Device_serial_number:
        offset = (unsigned int)(&system_configuration.device_serial_number) - (unsigned int)(&system_configuration);
        eeprom->read(&system_configuration.device_serial_number,address+offset,sizeof(system_configuration.device_serial_number));
        break;
    case Device_password:
        offset = (unsigned int)(&system_configuration.device_password) - (unsigned int)(&system_configuration);
        eeprom->read(&system_configuration.device_password,address+offset,sizeof(system_configuration.device_password));
        break;
    case Firmware_version:
        offset = (unsigned int)(&system_configuration.firmware_version) - (unsigned int)(&system_configuration);

        eeprom->read(&system_configuration.firmware_version,address+offset,sizeof(system_configuration.firmware_version));
        break;   
    case chip_id:
        offset = (unsigned int)(&system_configuration.chip_id) - (unsigned int)(&system_configuration);
        eeprom->read(&system_configuration.chip_id,address+offset,sizeof(system_configuration.chip_id));
        break; 
    case inputduty_switch:
        offset = (unsigned int)(&system_configuration.inputduty_switch) - (unsigned int)(&system_configuration);
        eeprom->read(&system_configuration.inputduty_switch,address+offset,sizeof(system_configuration.inputduty_switch));
        break; 
    case position_switch:
        offset = (unsigned int)(&system_configuration.position_switch) - (unsigned int)(&system_configuration);
        eeprom->read(&system_configuration.position_switch,address+offset,sizeof(system_configuration.position_switch));
        break; 
    case calibration_switch:
        offset = (unsigned int)(&system_configuration.calibration_switch) - (unsigned int)(&system_configuration);
        eeprom->read(&system_configuration.calibration_switch,address+offset,sizeof(system_configuration.calibration_switch));
        break; 
    case bluetooth_switch:
        offset = (unsigned int)(&system_configuration.bluetooth_switch) - (unsigned int)(&system_configuration);
        eeprom->read(&system_configuration.bluetooth_switch,address+offset,sizeof(system_configuration.bluetooth_switch));
        break; 


    case n_data_points:
        offset = (unsigned int)(&system_motion_characteristic_data.n_data_points) - (unsigned int)(&system_motion_characteristic_data);
        eeprom->read(&system_motion_characteristic_data.n_data_points,address + sizeof(system_configuration)+ offset,sizeof(system_motion_characteristic_data.n_data_points));
        break;          
    case x_data:
        offset = (unsigned int)(&system_motion_characteristic_data.x_data) - (unsigned int)(&system_motion_characteristic_data);
        eeprom->read(&system_motion_characteristic_data.x_data,address + sizeof(system_configuration) + offset,sizeof(system_motion_characteristic_data.x_data));
        break;  
    case y_data:
        offset = (unsigned int)(&system_motion_characteristic_data.y_data) - (unsigned int)(&system_motion_characteristic_data);
        eeprom->read(&system_motion_characteristic_data.y_data,address + sizeof(system_configuration) + offset,sizeof(system_motion_characteristic_data.y_data,address));
        break;   
    default:
        break;
    }
    eeprom->close();
    return true;
}

